#pragma once
#include <tf2_ros/transform_broadcaster.h>

#include <chrono>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>

#include "H1_Robot.hpp"

namespace util {
template <typename NodeT, typename T>
void param(NodeT node, const std::string& param_name, T& output_var,
           const T& default_value) {
  // 如果参数不存在，则声明并使用默认值
  if (!node->has_parameter(param_name)) {
    node->template declare_parameter<T>(param_name, default_value);
  }

  // 获取参数值
  node->get_parameter(param_name, output_var);

  // 输出日志，便于调试
  RCLCPP_INFO(
      node->get_logger(), "Parameter '%s' = %s", param_name.c_str(),
      rclcpp::Parameter(param_name, output_var).value_to_string().c_str());
}
}  // namespace util

class OdometryChassis : public rclcpp::Node {
 public:
  OdometryChassis();
  ~OdometryChassis();

 private:
  bool initialize();
  void initParamters();
  void initSubscribers();
  void initPublishers();

  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void updateLoop();
  void calcOdometry(double dt);
  void publishOdometry();
  void sendVelocity(double linear, double angular);
  void sendChassisHightCmd(double lin, double ang);
  void sendChassisLowCmd(double lin, double ang);
  double motorToWheelVelocity(double omega_motor_rad_s, double wheel_radius,
                              double gear_ratio = 1.0);
  void chassisVelToMotorSpeed(double v, double w, double& wl, double& wr);
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  H1Robot robot;
  std::chrono::steady_clock::time_point last_cmd_time_;
  std::chrono::steady_clock::time_point last_odom_time_;
  double yaw = 0.0;
  nav_msgs::msg::Odometry odom;
  bool publish_tf = true;
  bool low_level_control = false;
};
